import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2

class CameraPublisher(Node):
    def __init__(self):
        super().__init__('cv_publisher')
        self.publisher_ = self.create_publisher(Image, 'image_raw', 10)
        self.timer = self.create_timer(0.1, self.publish_image)  # 每 0.1 秒发布一次图像
        self.bridge = CvBridge()

        # 打开摄像头
        self.cap = cv2.VideoCapture('/dev/video21')
        if not self.cap.isOpened():
            self.get_logger().error('Failed to open camera')
            return

    def publish_image(self):
        ret, frame = self.cap.read()
        if ret:
            # 将 OpenCV 图像转换为 ROS 消息
            frame = cv2.resize(frame, (128, 128))
            ros_image = self.bridge.cv2_to_imgmsg(frame, encoding="bgr8")
            self.publisher_.publish(ros_image)
            self.get_logger().info('Publishing image')
            cv2.imshow('ori', frame)
            cv2.waitKey(1)
def main(args=None):
    rclpy.init(args=args)
    camera_publisher = CameraPublisher()

    rclpy.spin(camera_publisher)

    camera_publisher.cap.release()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

